#include "ImageEditor.h"
#include <cmath>

#define MAX(a, b) ((a) > (b) ? (a) : (b))
#define MIN(a, b) ((a) < (b) ? (a) : (b))
#define POS(x) MAX((x), 0)

// 阈值
#define THRESHOLD 1e-3

// 类型转换
CPoint trans(const Vector &p)
{
	return {(int)p[0], (int)p[1]};
}
Vector trans(CPoint p)
{
	return {(double)p.x, (double)p.y};
}

void IDW::warping(CImage *image1, CImage *image2, std::vector<CPoint> &pList, std::vector<CPoint> &qList)
{
	// 点的规模
	int n = pList.size();

	// 图像信息
	int width = image1->GetWidth();
	int height = image1->GetHeight();
	int N = width * height;

	// 获得进度条信息
	int count = 0;
	int lower = 0;
	int upper = 0;
	m_progress->GetRange(lower, upper);
	int range = upper - lower;

	// 存放各个控制点的权函数和插值函数
	std::vector<FuncX1> sigList(n);
	std::vector<FuncXX> interList(n);
	std::vector<double> rList(n);

	// 求解线性系统
	LinearSolver solver;

	// 获得插值函数并记录影响范围函数
	for (int i = 0; i < n; i++)
	{
		// 转化为向量方便操作
		Vector pi = trans(pList[i]);
		Vector qi = trans(qList[i]);

		// 影响范围，一般控制点的影响范围是到距离最近的点的长度的两倍
		// 最后一点是锚定点，为了确保所有图像点至少在一个控制点的影响范围内
		rList[i] = (i < n - 1) ? (radius(i, pList) * 2) : (distance({width, height}, {0, 0}) / 2 + 1);

		// 构建 sigma 函数
		sigList[i] = sigma(pi, rList[i], 2);

		// 通过最小化误差函数，获得局部插值函数
		Matrix A(4, 4);
		Vector b(4);

		// 考虑 Akl 元素的系数，构造线性方程组
		for (int k = 0; k < 2; k++)
		{
			for (int l = 0; l < 2; l++)
			{
				// 这是第 ind 个方程
				int ind = 2 * k + l;

				for (int j = 0; j < n; j++)
				{
					// 跳过 i
					if (j == i)
						continue;

					// 转化为向量方便操作
					Vector pj = trans(pList[j]);
					Vector qj = trans(qList[j]);

					// 纵向索引由 k 确定
					double sig = sigList[i](pj);
					A(ind, k * 2) += sig * (pj[l] - pi[l]) * (pj[0] - pi[0]);
					A(ind, k * 2 + 1) += sig * (pj[l] - pi[l]) * (pj[1] - pi[1]);
					b[ind] += sig * (qj[k] - qi[k]) * (pj[l] - pi[l]);
				}
			}
		}

		// 为了防止奇异情形
		b = A.transpose() * b;
		A = A.transpose() * A;
		Vector x = solver.solve(A, b, true);

		// 结果存放到矩阵
		Matrix Ti(2, 2, {x[0], x[1], x[2], x[3]});

		// 插值得到函数并保存
		interList[i] = interpolate(Ti, pi, qi);
	}

	// 开始映射
	for (int i = 0; i < height; i++)
	{
		for (int j = 0; j < width; j++)
		{
			// 获得当前点位置
			Vector p = {double(j), double(i)};
			Vector np(2);

			// 先计算权的总和
			double s = weight(sigList, p);

			// 只计算影响当前点的映射
			for (int k = 0; k < n; k++)
			{
				if (distance(trans(p), pList[k]) < rList[k] + 1)
				{
					// 当前权和对应的值
					double w = sigList[k](p);
					Vector value = interList[k](p);

					// 如果 w 是 inf，说明当前权为 1
					if (std::isinf(w))
					{
						np = np + value;
						break;
					}
					// 否则如果 s 是 inf，说明权为 0
					else if (std::isinf(s))
					{
						// 什么都不做
					}
					// 否则计算占比
					else
						np = np + value * w / s;
				}
			}

			// 获取对应点
			CPoint res = trans(np);

			// 使用的点的坐标位置 (i,j)，修改点值
			m_image = image1;
			PIMAGE pImg = this->getPixel(i, j);

			m_image = image2;
			this->setPixel(res.y, res.x, pImg);

			// 修改进度
			m_progress->SetPos(1.0 * range * count / N + lower);
			count++;
		}
	}
}

// RBF 变形
void RBF::warping(CImage *image1, CImage *image2, std::vector<CPoint> &pList, std::vector<CPoint> &qList)
{
	int n = pList.size();

	// 图像信息
	int width = image1->GetWidth();
	int height = image1->GetHeight();
	int N = width * height;

	// 获得进度条信息
	int count = 0;
	int lower = 0;
	int upper = 0;
	m_progress->GetRange(lower, upper);
	int range = upper - lower;

	// 插值函数所需的所有系数
	std::vector<Vector> aList(n, Vector(2));
	std::vector<double> rList(n);
	std::vector<double> dList(n);

	// 首先计算特征半径
	for (int i = 0; i < n; i++)
		rList[i] = radius(i, pList);

	// 构建系数矩阵 D 和 b
	Matrix D(n, n);
	Vector bx(n), by(n);
	for (int i = 0; i < n; i++)
	{
		for (int j = 0; j < n; j++)
			D(i, j) = basis(distance(pList[i], pList[j]), rList[j], 0);

		CPoint p = qList[i] - pList[i];
		bx[i] = p.x;
		by[i] = p.y;
	}

	// 使用 Gauss-Seidel 迭代求解估计 alpha
	LinearSolver solver;
	Vector x = solver.GaussSediel(D, bx);
	Vector y = solver.GaussSediel(D, by);
	for (int i = 0; i < n; i++)
	{
		aList[i][0] = x[i];
		aList[i][1] = y[i];
	}

	// 计算 delta
	for (int i = 0; i < n; i++)
		dList[i] = THRESHOLD / MAX(fabs(aList[i][0]), fabs(aList[i][1]));

	// 最后再构建系统重新求解 alpha
	for (int i = 0; i < n; i++)
		for (int j = 0; j < n; j++)
			D(i, j) = basis(distance(pList[i], pList[j]), rList[j], dList[j]);

	x = solver.GaussSediel(D, bx);
	y = solver.GaussSediel(D, by);
	for (int i = 0; i < n; i++)
	{
		aList[i][0] = x[i];
		aList[i][1] = y[i];
	}

	// 开始插值
	for (int i = 0; i < height; i++)
	{
		for (int j = 0; j < width; j++)
		{
			// 获得当前点位置
			Vector p = {double(j), double(i)};
			Vector np = p;

			// 只计算影响当前点的映射
			for (int k = 0; k < n; k++)
			{
				double d = distance(trans(p), pList[k]);
				if (d < radius(dList[k], rList[k]) + 1)
					np = np + aList[k] * basis(d, rList[k], dList[k]);
			}

			// 获取对应点
			CPoint res = trans(np);

			// 使用的点的坐标位置 (i,j)
			m_image = image1;
			PIMAGE pImg = this->getPixel(i, j);

			m_image = image2;
			this->setPixel(res.y, res.x, pImg);

			// 修改进度
			m_progress->SetPos(1.0 * range * count / N + lower);
			count++;
		}
	}
}

// 特征半径，即 ri = min_{j neq i}di(pj)
double radius(int i, std::vector<CPoint> &p)
{
	double ri = FLT_MAX;
	for (int j = 0; j < p.size(); j++)
	{
		if (j == i)
			continue;

		ri = MIN(ri, distance(p[i], p[j]));
	}
	return ri;
}

// 计算两点的距离
double distance(CPoint p1, CPoint p2)
{
	return sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2));
}

// 基函数，使用 mu = -1
double basis(double d, double ri, double delta)
{
	return POS(1 / (d * d + ri * ri) - delta);
}

// 计算影响半径
double radius(double delta, double ri)
{
	return sqrt(1 / delta - ri * ri);
}

// sigma_i(p)
FuncX1 sigma(Vector &pi, double Ri, double mu)
{
	FuncX1 f = [=](Vector p) -> Real
	{
		double dis = distance(trans(p), trans(pi));
		return pow(POS(Ri - dis) / (Ri * dis), mu);
	};
	return f;
}

// 计算权的总和
double weight(std::vector<FuncX1> &sigList, Vector &p)
{
	double s = 0;
	for (int j = 0; j < sigList.size(); j++)
		s += sigList[j](p);

	return s;
}

// 控制点插值函数
FuncXX interpolate(Matrix &Ti, Vector &pi, Vector &qi)
{
	FuncXX f = [=](Vector p) -> Vector
	{
		return qi + Ti * (p - pi);
	};
	return f;
}